#include <rov/sensors.h>
#include <rov/util.h>

#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <errno.h>

#include <iostream>
#include <sstream>


using namespace std;


#define BAUD_RATE	B57600


static char const * const IMU_ID = "imu";
static char const * const DEVICE_PATTERN = "/dev/ttyACM";
static int imuFd = -1;


Sensor initImu()
{
    int fd = -1;

    while (fd < 0)
    {
        for (int i = 0; i < 10 && fd < 0; i++)
        {
            stringstream name;

            name << DEVICE_PATTERN << i;

cerr << "checking device: " << name.str() << endl;

            fd = open(name.str().c_str(), O_RDWR | O_NOCTTY);

            if (fd >= 0 && (configSerial(fd, BAUD_RATE)
                            || !checkMatches(fd, IMU_ID)))
            {
                close(fd);
                fd = -1;
            }
        }

        sleep(1);
    }

    if (fd >= 0)
    {
        errno = 0;
        imuFd = fd;
    }

    return fd;
}
